﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Diagnostics;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;
using MessageBox = System.Windows.Forms.MessageBox;

// WPF
using System.Windows;
using System.Windows.Controls;
using System.Windows.Shapes;
using System.Windows.Media;
using Brush = System.Windows.Media.Brush;
using Brushes = System.Windows.Media.Brushes;
using Color = System.Windows.Media.Color;
using Point = System.Windows.Point;
using Rectangle = System.Windows.Shapes.Rectangle;

// Project defined usings
using Kinect_Interface_Prototype_1.Mouse_Control;
using Kinect_Interface_Prototype_1.Interface_Design;
using Kinect_Interface_Prototype_1.Folder_Control;
using Kinect_Interface_Prototype_1.Voice_Commands;
using Kinect_Interface_Prototype_1.Gesture_Control;

namespace Kinect_Interface_Prototype_1
{
    public partial class KinectForm : Form
    {
        private StartupProgram MainProgram;
        private Canvas skeletonCanvas;
        private GestureRectangle StartGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.StartGestureRectangle]);
        private GestureRectangle RightClickGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.RightClickGestureRectangle]);
        private GestureRectangle UpperGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.UpperGestureRectangle]);
        private GestureRectangle LeftClickGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LeftClickGestureRectangle]);
        private GestureRectangle RightHandSwipeGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.RightHandSwipeGestureRectangle]);
        private GestureRectangle LeftHandSwipeGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LeftHandSwipeGestureRectangle]);
        private GestureRectangle CenterGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.CenterGestureRectangle]);
        private GestureRectangle LowerLeftGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LowerLeftGestureRectangle]);
        private GestureRectangle UpperRightGestureRectangle = new GestureRectangle(GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.UpperRightGestureRectangle]);

       // public GestureControl gesture_control;
        private Dictionary<GestureRectangleType, GestureRectangle> GestureRectangleDictionary;

        public enum ImageType { Color, Depth }

        private IniFile settingsINI = new IniFile(Constants.SettingsINI);

        private const float SkeletonMaxX = 0.60f;
        private const float SkeletonMaxY = 0.40f;

        private KinectSensor kinectSensor;
        private Skeleton[] skeletons;
        private Brush brush = new SolidColorBrush(Color.FromRgb(128, 128, 255));
        private int totalFrames = 0;
        private int lastFrames = 0;
        private DateTime lastTime = DateTime.MaxValue;

        private bool MouseControlOn = true;
        private bool GestureControlOn = true;

        private Dictionary<JointType, Brush> jointColours = new Dictionary<JointType, Brush>()
        {
            { JointType.HipCenter, new SolidColorBrush(Color.FromRgb(169, 176, 155)) },
            { JointType.Spine, new SolidColorBrush(Color.FromRgb(169, 176, 155)) },
            { JointType.ShoulderCenter, new SolidColorBrush(Color.FromRgb(168, 230, 29)) },
            { JointType.Head, new SolidColorBrush(Color.FromRgb(200, 0,   0)) },
            { JointType.ShoulderLeft, new SolidColorBrush(Color.FromRgb(79,  84,  33)) },
            { JointType.ElbowLeft, new SolidColorBrush(Color.FromRgb(84,  33,  42)) },
            { JointType.WristLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0)) },
            { JointType.HandLeft, new SolidColorBrush(Color.FromRgb(215,  86, 0)) },
            { JointType.ShoulderRight, new SolidColorBrush(Color.FromRgb(33,  79,  84)) },
            { JointType.ElbowRight, new SolidColorBrush(Color.FromRgb(33,  33,  84)) },
            { JointType.WristRight, new SolidColorBrush(Color.FromRgb(77,  109, 243)) },
            { JointType.HandRight, new SolidColorBrush(Color.FromRgb(37,   69, 243)) },
            { JointType.HipLeft, new SolidColorBrush(Color.FromRgb(77,  109, 243)) },
            { JointType.KneeLeft, new SolidColorBrush(Color.FromRgb(69,  33,  84)) },
            { JointType.AnkleLeft, new SolidColorBrush(Color.FromRgb(229, 170, 122)) },
            { JointType.FootLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0)) },
            { JointType.HipRight, new SolidColorBrush(Color.FromRgb(181, 165, 213)) },
            { JointType.KneeRight, new SolidColorBrush(Color.FromRgb(71, 222,  76)) },
            { JointType.AnkleRight, new SolidColorBrush(Color.FromRgb(245, 228, 156)) },
            { JointType.FootRight, new SolidColorBrush(Color.FromRgb(77,  109, 243)) }
        };

        //Use to transform and reduce jitter
        private TransformSmoothParameters TransformParameters = new TransformSmoothParameters
        {
            Smoothing = 0.1f,
            Correction = 0.3f,
            Prediction = 0.4f,
            JitterRadius = 1.0f,
            MaxDeviationRadius = 0.5f
        };

        public KinectForm(StartupProgram program)
        {
            InitializeComponent();
            MainProgram = program;
            MainProgram.AddSystemButtons(this); // Add the close, maximize, and minimize buttons

            skeletonCanvas = new Canvas();
            skeletonCanvas.Width = 400;
            skeletonCanvas.Height = 300;
            skeletonCanvas.Background = Brushes.Black;
            skeletonCanvas.Margin = new Thickness(10, 0, 10, 10);

            canvasHost.Child = skeletonCanvas;

            GestureRectangleDictionary = new Dictionary<GestureRectangleType, GestureRectangle>
            {
                {GestureRectangleType.StartGestureRectangle, StartGestureRectangle},
                {GestureRectangleType.RightClickGestureRectangle, RightClickGestureRectangle},
                {GestureRectangleType.UpperGestureRectangle, UpperGestureRectangle},
                {GestureRectangleType.LeftClickGestureRectangle, LeftClickGestureRectangle},
                {GestureRectangleType.RightHandSwipeGestureRectangle, RightHandSwipeGestureRectangle},
                {GestureRectangleType.LeftHandSwipeGestureRectangle, LeftHandSwipeGestureRectangle},
                {GestureRectangleType.CenterGestureRectangle, CenterGestureRectangle}
            };

            try
            {
                foreach (KinectSensor sensor in KinectSensor.KinectSensors)
                {
                    if (sensor.Status == KinectStatus.Connected)
                    {
                        // Pass parameters to set smoothing
                        sensor.SkeletonStream.Enable(TransformParameters);
                        sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                        sensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
                        sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(KinectAllFramesReady);
                        sensor.DepthStream.Range = DepthRange.Default;
                        sensor.Start();

                        this.kinectSensor = sensor;

                        break;
                    }
                }
            }
            catch (InvalidOperationException)
            {
                MessageBox.Show("Runtime initialization failed. Ensure Kinect is plugged in");
                return;
            }

            GestureControlOn = Boolean.Parse(settingsINI.IniReadValue("General", "gestureControlOnStart"));
            MouseControlOn = Boolean.Parse(settingsINI.IniReadValue("General", "mouseControlOnStart"));

            this.lastTime = DateTime.Now;
        }

        private void KinectAllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                this.skeletonCanvas.Children.Clear();

                bool receivedData = false;

                if (skeletonFrame != null)
                {
                    if (skeletons == null) //allocate the first time
                    {
                        skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    }

                    skeletonFrame.CopySkeletonDataTo(skeletons);

                    receivedData = true;
                }

                if (receivedData)
                {
                    // Gets closest skeleton (Only tracks one)
                    List<Skeleton> trackedSkeletons = skeletons.Where(s => s != null && s.TrackingState == SkeletonTrackingState.Tracked).ToList();
                    Skeleton skeletalData = trackedSkeletons.Count > 0 ? trackedSkeletons.Aggregate((closest, s) => (closest == null || (s != null && (s.Position.Z) < closest.Position.Z) ? s : closest)) : null;
                                        

                    // If no tracked skeleton, switch skeletal tracking to off and return.
                    if (skeletalData == null)
                    {
                        MainProgram.ControlForm_SwitchOff(OnOffControlType.SkeletalTracking);
                        return;
                    }

                    // If the skeletal tracking switch isn't already on, switch it to on
                    if (!MainProgram.ControlForm_GetSwitchOn(OnOffControlType.SkeletalTracking))
                        MainProgram.ControlForm_SwitchOn(OnOffControlType.SkeletalTracking);

                    using (DepthImageFrame depthImageFrame = e.OpenDepthImageFrame())
                    {
                        if (depthImageFrame != null)
                        {
                            // Draw bones
                            this.skeletonCanvas.Children.Add(this.getBodySegment(depthImageFrame, skeletalData, brush, JointType.HipLeft, JointType.HipCenter, JointType.HipRight));
                            this.skeletonCanvas.Children.Add(this.getBodySegment(depthImageFrame, skeletalData, brush, JointType.HipCenter, JointType.Spine, JointType.ShoulderCenter, JointType.Head));
                            this.skeletonCanvas.Children.Add(this.getBodySegment(depthImageFrame, skeletalData, brush, JointType.ShoulderCenter, JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, JointType.HandLeft));
                            this.skeletonCanvas.Children.Add(this.getBodySegment(depthImageFrame, skeletalData, brush, JointType.ShoulderCenter, JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, JointType.HandRight));

                            var drawJoints = new List<Joint>(){skeletalData.Joints[JointType.HipCenter], skeletalData.Joints[JointType.HipLeft], skeletalData.Joints[JointType.HipRight], 
                                                skeletalData.Joints[JointType.Spine], skeletalData.Joints[JointType.ShoulderCenter], skeletalData.Joints[JointType.Head], skeletalData.Joints[JointType.ShoulderLeft],
                                                skeletalData.Joints[JointType.ElbowLeft], skeletalData.Joints[JointType.WristLeft], skeletalData.Joints[JointType.HandLeft], skeletalData.Joints[JointType.ShoulderRight], 
                                                skeletalData.Joints[JointType.ElbowRight], skeletalData.Joints[JointType.WristRight], skeletalData.Joints[JointType.HandRight]};

                            // Draw joints
                            foreach (Joint joint in drawJoints)
                            {
                                Point jointPos = GetPosition2DLocation(depthImageFrame, joint.Position);
                                Line jointLine = new Line();
                                jointLine.X1 = jointPos.X - 3;
                                jointLine.X2 = jointLine.X1 + 6;
                                jointLine.Y1 = jointLine.Y2 = jointPos.Y;
                                jointLine.Stroke = jointColours[joint.JointType];
                                jointLine.StrokeThickness = 6;

                                this.skeletonCanvas.Children.Add(jointLine);
                            }

                            Joint rightHand = skeletalData.Joints[JointType.HandRight];
                            Joint leftHand = skeletalData.Joints[JointType.HandLeft];
                            Joint shoulderLeft = skeletalData.Joints[JointType.ShoulderLeft];
                            Joint shoulderRight = skeletalData.Joints[JointType.ShoulderRight];
                            Joint hipLeft = skeletalData.Joints[JointType.HipLeft];

                            float minDepth = Math.Min(rightHand.Position.Z, Math.Min(leftHand.Position.Z, Math.Min(shoulderLeft.Position.Z, hipLeft.Position.Z))) * 1000;

                            Point rightHandPoint = GetPosition2DLocation(depthImageFrame, rightHand.Position);
                            Point leftHandPoint = GetPosition2DLocation(depthImageFrame, skeletalData.Joints[JointType.HandLeft].Position);
                            Point shoulderLeftPoint = GetPosition2DLocation(depthImageFrame, skeletalData.Joints[JointType.ShoulderLeft].Position);
                            Point shoulderRightPoint = GetPosition2DLocation(depthImageFrame, skeletalData.Joints[JointType.ShoulderRight].Position);
                            Point hipLeftPoint = GetPosition2DLocation(depthImageFrame, skeletalData.Joints[JointType.HipLeft].Position);

                            DrawGestureRectangles(rightHandPoint, leftHandPoint, shoulderLeftPoint, shoulderRightPoint, hipLeftPoint, shoulderLeft.Position.Z, shoulderRight.Position.Z, hipLeft.Position.Z);

                            rightHandDepth.Text = rightHand.Position.Z * 1000 + " mm";
                            leftShoulderDepth.Text = shoulderLeft.Position.Z * 1000 + " mm";
                            leftHipDepth.Text = hipLeft.Position.Z * 1000 + " mm"; ;
                            leftHandDepth.Text = leftHand.Position.Z * 1000 + " mm"; ;
                            rightHandX.Text = rightHandPoint.X.ToString();
                            rightHandY.Text = rightHandPoint.Y.ToString();

                            if (MouseControlOn)
                            {
                                MainProgram.MouseControl_UpdateMouse(minDepth, rightHand, SkeletonMaxX, SkeletonMaxY);
                            }

                            if (GestureControlOn)
                            {
                                MainProgram.GestureControl_DetermineGestures(MouseControlOn, GestureControlOn, minDepth, StartGestureRectangle.IsInside,
                                                                             LeftClickGestureRectangle.IsInside, UpperGestureRectangle.IsInside, RightClickGestureRectangle.IsInside,
                                                                             RightHandSwipeGestureRectangle.IsInside, RightHandSwipeGestureRectangle.HandInside,
                                                                             LeftHandSwipeGestureRectangle.IsInside, LeftHandSwipeGestureRectangle.HandInside,
                                                                             CenterGestureRectangle.IsInside, CenterGestureRectangle.HandInside, LowerLeftGestureRectangle.IsInside,
                                                                             LowerLeftGestureRectangle.HandInside, UpperRightGestureRectangle.IsInside, UpperRightGestureRectangle.HandInside);

                                MainProgram.UpdateVirtualKeyboard(leftHand, rightHand);
                            }
                        }
                    }
                }
            }
            this.CalculateFps();
        }

        private void DrawGestureRectangles(Point rightHandPoint, Point leftHandPoint, Point shoulderLeftPoint, Point shoulderRightPoint, Point hipLeftPoint,
                                            float shoulderLeftDepth, float shoulderRightDepth, float hipLeftDepth)
        {
            DrawStartGestureRectangle(leftHandPoint, shoulderLeftPoint, shoulderLeftDepth);
            DrawRightClickGestureRectangle(leftHandPoint, shoulderLeftPoint, hipLeftPoint, shoulderLeftDepth, hipLeftDepth);
            DrawUpperGestureRectangle(leftHandPoint, shoulderLeftPoint, shoulderLeftDepth);
            DrawLeftClickGestureRectangle(leftHandPoint, hipLeftPoint, hipLeftDepth);
            DrawRightHandSwipeGestureRectangle(rightHandPoint, leftHandPoint, hipLeftPoint, hipLeftDepth);
            DrawLeftHandSwipeGestureRectangle(rightHandPoint, leftHandPoint, hipLeftPoint, hipLeftDepth);
            DrawCenterGestureRectangle(rightHandPoint, leftHandPoint, hipLeftPoint, hipLeftDepth);
            DrawLowerLeftGestureRectangle(leftHandPoint, hipLeftPoint, hipLeftDepth);
            DrawUpperRightGestureRectangle(rightHandPoint, shoulderRightPoint, shoulderRightDepth);
        }

        private void DrawStartGestureRectangle(Point leftHandPoint, Point shoulderLeftPoint, float shoulderLeftDepth)
        {
            shoulderLeftDepth = (shoulderLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.StartGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (shoulderLeftDepth / 40));
            int Ymod = (int)(intiParams.Ymod - (shoulderLeftDepth / 45));
            int width = (int)(intiParams.Width + (shoulderLeftDepth / 200));
            int height = shoulderLeftDepth > 0 ? (int)(intiParams.Height - (shoulderLeftDepth / 80)) : intiParams.Height;

            StartGestureRectangle.SetParameters(shoulderLeftPoint, width, height, Xmod, Ymod);
            StartGestureRectangle.DetermineIfLeftInside(leftHandPoint);

            if (StartGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(StartGestureRectangle.Rectangle);
        }

        private void DrawRightClickGestureRectangle(Point leftHandPoint, Point shoulderLeftPoint, Point hipLeftPoint, float shoulderLeftDepth, float hipLeftDepth)
        {
            shoulderLeftDepth = (shoulderLeftDepth * 1000) - 1700;
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.RightClickGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (shoulderLeftDepth / 40));
            int Ymod = (int)(intiParams.Ymod + (hipLeftDepth / 80));
            int width = (int)(intiParams.Width - (shoulderLeftDepth / 180));
            int height = (int)(intiParams.Height - (hipLeftDepth / 180));

            RightClickGestureRectangle.SetParameters(shoulderLeftPoint.X, hipLeftPoint.Y, width, height, Xmod, Ymod);
            RightClickGestureRectangle.DetermineIfLeftInside(leftHandPoint);

            if (RightClickGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(RightClickGestureRectangle.Rectangle);
        }

        private void DrawUpperGestureRectangle(Point leftHandPoint, Point shoulderLeftPoint, float shoulderLeftDepth)
        {
            shoulderLeftDepth = (shoulderLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.UpperGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (shoulderLeftDepth / 40));
            int Ymod = (int)(intiParams.Ymod - (shoulderLeftDepth / 45));
            int width = (int)(intiParams.Width + (shoulderLeftDepth / 200));
            int height = shoulderLeftDepth > 0 ? (int)(intiParams.Height - (shoulderLeftDepth / 140)) : intiParams.Height;

            UpperGestureRectangle.SetParameters(shoulderLeftPoint, width, height, Xmod, Ymod);
            UpperGestureRectangle.DetermineIfLeftInside(leftHandPoint);

            if (UpperGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(UpperGestureRectangle.Rectangle);
        }

        private void DrawLeftClickGestureRectangle(Point leftHandPoint, Point hipLeftPoint, float hipLeftDepth)
        {
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LeftClickGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (hipLeftDepth / 20));
            int Ymod = (int)(intiParams.Ymod - (hipLeftDepth / 23));
            int width = (int)(intiParams.Width + (hipLeftDepth / 180));
            int height = (int)(intiParams.Height + (hipLeftDepth / 180));

            LeftClickGestureRectangle.SetParameters(hipLeftPoint, width, height, Xmod, Ymod);
            LeftClickGestureRectangle.DetermineIfLeftInside(leftHandPoint);

            if (LeftClickGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(LeftClickGestureRectangle.Rectangle);
        }

        private void DrawRightHandSwipeGestureRectangle(Point rightHandPoint, Point leftHandPoint, Point hipLeftPoint, float hipLeftDepth)
        {
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.RightHandSwipeGestureRectangle];

            int Xmod = (int)(intiParams.Xmod + (hipLeftDepth / 70));
            int Ymod = (int)(intiParams.Ymod - (hipLeftDepth / 20));
            int width = hipLeftDepth > 0 ? (int)(intiParams.Width - (hipLeftDepth / 120)) : intiParams.Width;
            int height = hipLeftDepth > 0 ? (int)(intiParams.Height - (hipLeftDepth / 80)) : intiParams.Height;

            RightHandSwipeGestureRectangle.SetParameters(hipLeftPoint, width, height, Xmod, Ymod);
            RightHandSwipeGestureRectangle.DetermineIfInside(rightHandPoint, leftHandPoint);

            if (RightHandSwipeGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(RightHandSwipeGestureRectangle.Rectangle);
        }

        private void DrawLeftHandSwipeGestureRectangle(Point rightHandPoint, Point leftHandPoint, Point hipLeftPoint, float hipLeftDepth)
        {
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LeftHandSwipeGestureRectangle];

            int Xmod = (int)(intiParams.Xmod + (hipLeftDepth / 70));
            int Ymod = (int)(intiParams.Ymod - (hipLeftDepth / 20));
            int width = hipLeftDepth > 0 ? (int)(intiParams.Width - (hipLeftDepth / 120)) : intiParams.Width;
            int height = hipLeftDepth > 0 ? (int)(intiParams.Height - (hipLeftDepth / 80)) : intiParams.Height;

            LeftHandSwipeGestureRectangle.SetParameters(hipLeftPoint, width, height, Xmod, Ymod);
            LeftHandSwipeGestureRectangle.DetermineIfInside(rightHandPoint, leftHandPoint);

            if (LeftHandSwipeGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(LeftHandSwipeGestureRectangle.Rectangle);
        }

        private void DrawCenterGestureRectangle(Point rightHandPoint, Point leftHandPoint, Point hipLeftPoint, float hipLeftDepth)
        {
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.CenterGestureRectangle];

            int Xmod = (int)(intiParams.Xmod + (hipLeftDepth / 70));
            int Ymod = (int)(intiParams.Ymod - (hipLeftDepth / 20));
            int width = hipLeftDepth > 0 ? (int)(intiParams.Width - (hipLeftDepth / 120)) : intiParams.Width;
            int height = hipLeftDepth > 0 ? (int)(intiParams.Height - (hipLeftDepth / 80)) : intiParams.Height;

            CenterGestureRectangle.SetParameters(hipLeftPoint, width, height, Xmod, Ymod);
            CenterGestureRectangle.DetermineIfBothInside(rightHandPoint, leftHandPoint); // Determines if both hands are inside

            if (CenterGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(CenterGestureRectangle.Rectangle);
        }

        private void DrawLowerLeftGestureRectangle(Point leftHandPoint, Point hipLeftPoint, float hipLeftDepth)
        {
            hipLeftDepth = (hipLeftDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.LowerLeftGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (hipLeftDepth / 50));
            int Ymod = (int)(intiParams.Ymod - (hipLeftDepth / 50));
            int width = (int)(intiParams.Width + (hipLeftDepth / 180));
            int height = (int)(intiParams.Height + (hipLeftDepth / 180));

            LowerLeftGestureRectangle.SetParameters(hipLeftPoint, width, height, Xmod, Ymod);
            LowerLeftGestureRectangle.DetermineIfLeftInside(leftHandPoint);

            if (LowerLeftGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(LowerLeftGestureRectangle.Rectangle);
        }

        private void DrawUpperRightGestureRectangle(Point rightHandPoint, Point shoulderRightPoint, float shoulderRightDepth)
        {
            shoulderRightDepth = (shoulderRightDepth * 1000) - 1700;
            GestureRectInitialParams intiParams = GestureRectInitialParams.initialParamsDictionary[GestureRectangleType.UpperRightGestureRectangle];

            int Xmod = (int)(intiParams.Xmod - (shoulderRightDepth / 50));
            int Ymod = (int)(intiParams.Ymod - (shoulderRightDepth / 50));
            int width = (int)(intiParams.Width + (shoulderRightDepth / 180));
            int height = (int)(intiParams.Height + (shoulderRightDepth / 180));

            UpperRightGestureRectangle.SetParameters(shoulderRightPoint, width, height, Xmod, Ymod);
            UpperRightGestureRectangle.DetermineIfRightInside(rightHandPoint);

            if (UpperRightGestureRectangle.Active)
                this.skeletonCanvas.Children.Add(UpperRightGestureRectangle.Rectangle);
        }

        private Polyline getBodySegment(DepthImageFrame depthFrame, Skeleton skeletalData, Brush brush, params JointType[] jointTypes)
        {
            PointCollection points = new PointCollection(jointTypes.Length);
            foreach (var jointType in jointTypes)
            {
                points.Add(GetPosition2DLocation(depthFrame, skeletalData.Joints[jointType].Position));
            }
            return new Polyline()
            {
                Points = points,
                Stroke = brush,
                StrokeThickness = 5
            };
        }

        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint)
        {
            ImageType ImageType = ImageType.Depth;

            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPoint);

            switch (ImageType)
            {
                case ImageType.Color:
                    ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, kinectSensor.ColorStream.Format);

                    // map back to skeleton.Width & skeleton.Height
                    return new Point(
                        (int)(skeletonCanvas.ActualWidth * colorPoint.X / kinectSensor.ColorStream.FrameWidth),
                        (int)(skeletonCanvas.ActualHeight * colorPoint.Y / kinectSensor.ColorStream.FrameHeight));
                case ImageType.Depth:
                    double depthX = Math.Max(0, Math.Min(skeletonCanvas.ActualWidth * depthPoint.X / depthFrame.Width, skeletonCanvas.ActualWidth));
                    double depthY = Math.Max(0, Math.Min(skeletonCanvas.ActualHeight * depthPoint.Y / depthFrame.Height, skeletonCanvas.ActualHeight));
                    return new Point((int)depthX, (int)depthY);
                default:
                    throw new ArgumentOutOfRangeException("ImageType was a not expected value: " + ImageType.ToString());
            }
        }

        private void CalculateFps()
        {
            this.totalFrames++;
            DateTime current = DateTime.Now;
            if (current.Subtract(this.lastTime) > TimeSpan.FromSeconds(1))
            {
                int frameDifference = this.totalFrames - this.lastFrames;
                this.lastFrames = this.totalFrames;
                this.lastTime = current;
                this.fps.Text = frameDifference.ToString();
            }
        }

        private void KinectForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (e.CloseReason == CloseReason.UserClosing)
            {
                // Cancel the close and hide the form
                e.Cancel = true;
                MainProgram.KinectForm_ToggleSkeletalWindowVisibility(false);
            }
        }

        // Public Functions
        public void AdjustElevation(int elevation)
        {
            if (kinectSensor == null) return;

            if (elevation > kinectSensor.MinElevationAngle && elevation < kinectSensor.MaxElevationAngle)
            {
                kinectSensor.ElevationAngle = elevation;
            }
        }

        public void ShutdownKinect()
        {
            if (kinectSensor != null) kinectSensor.Stop();
        }

        public void ToggleMouseControl(bool active)
        {
            MouseControlOn = active;
        }

        public void ToggleGestureControl(bool active)
        {
            GestureControlOn = active;
        }

        public void ToggleActiveGestureRectangle(GestureRectangleType type, bool active)
        {
            GestureRectangleDictionary[type].Active = active;
        }
    }   
}
